#ifndef _UNIFORM_BSPLINE_H_
#define _UNIFORM_BSPLINE_H_

#include <Eigen/Eigen>
#include <algorithm>
#include <iostream>

using namespace std;

namespace ego_planner {
// An implementation of non-uniform B-spline with different dimensions
// It also represents uniform B-spline which is a special case of non-uniform
class UniformBspline
{
private:
  // control points for B-spline with different dimensions.
  // Each row represents one single control point
  // The dimension is determined by column number
  // e.g. B-spline with N points in 3D space -> Nx3 matrix
  Eigen::MatrixXd control_points_;

  int p_;             // p degree
  int n_;             // n+1 control points
  int m_;             // m = n+p+1, m+1 knots
  double interval_;   // knot span \delta t
  Eigen::VectorXd u_; // knots vector, record time information

  Eigen::MatrixXd getDerivativeControlPoints();

  // physical limits and time adjustment ratio
  double limit_vel_, limit_acc_, limit_ratio_, feasibility_tolerance_;

public:
  UniformBspline() {}
  UniformBspline(const Eigen::MatrixXd &points, const int &order, const double &interval)
  {
    control_points_ = points;
    p_ = order;
    interval_ = interval;

    n_ = points.cols() - 1;
    m_ = n_ + p_ + 1;

    u_ = Eigen::VectorXd::Zero(m_ + 1);
    for (int i = 0; i <= m_; ++i)
    {
      if (i <= p_)
      {
        u_(i) = double(-p_ + i) * interval_;
      }
      else if (i > p_ && i <= m_ - p_)
      {
        u_(i) = u_(i - 1) + interval_;
      }
      else if (i > m_ - p_) // 好像和上面没区别
      {
        u_(i) = u_(i - 1) + interval_;
      }
    }
  }

  ~UniformBspline(){};

  // get / set basic bspline info
  void setKnot(const Eigen::VectorXd &knot) { u_ = knot; };
  double getInterval() { return interval_; }
  Eigen::VectorXd getKnot() { return u_; };
  Eigen::MatrixXd getControlPoint() { return control_points_; }
  bool getTimeSpan(double &um, double &um_p);
  double getControlPtMaxT(int index);
  static double getControlPtMaxT(int index, int degree, double interval);

  // compute position / derivative

  // use u , in [up, u_mp]
  Eigen::VectorXd getPos(const double &u);
  // use t , in [0, duration]
  Eigen::VectorXd getPosT(const double &t) { return getPos(t + u_(p_)); }
  UniformBspline getDerivative();

  static void BsplineFitting(const double &dt,
                             const vector<Eigen::Vector3d> &pts_init,
                             const vector<Eigen::Vector3d> &terminal_constraint,
                             Eigen::MatrixXd &pts_ctrl);

  /* check feasibility, adjust time */

  void setPhysicalLimits(const double &vel, const double &acc, const double &tolerance);
  bool checkFeasibility(double &ratio, bool show = false);
  void lengthenTime(const double &ratio);

  /* for performance evaluation */

  double getTimeSum();
  double getLength(const double &res = 0.01);
  double getJerk();
  void getMeanAndMaxVel(double &mean_v, double &max_v);
  void getMeanAndMaxAcc(double &mean_a, double &max_a);

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

/* -------------------------------------------------------------------------- */
/*                         inline function definition                         */
/* -------------------------------------------------------------------------- */
inline bool UniformBspline::getTimeSpan(double &um, double &um_p)
{
  // TODO 这不可能，可以删除
  if (p_ > u_.rows() || m_ - p_ > u_.rows()) {
    cerr << "\033[1;31m[Bspline]:p_ > u_.rows() || m_ - p_ > u_.rows() \033[0m"
         << endl;
    return false;
  }

  um = u_(p_);
  um_p = u_(m_ - p_);

  return true;
}

inline double UniformBspline::getTimeSum()
{
  double tm, tmp;
  if (getTimeSpan(tm, tmp))
    return tmp - tm;
  else
    return -1.0;
}

inline double UniformBspline::getLength(const double &res)
{
  double length = 0.0;
  double dur = getTimeSum();
  Eigen::VectorXd p_l = getPosT(0.0), p_n;
  for (double t = res; t <= dur + 1e-4; t += res)
  {
    p_n = getPosT(t);
    length += (p_n - p_l).norm();
    p_l = p_n;
  }
  return length;
}

/**
 * 返回第index个控制点对应的b样条基函数最大的时间点 index = 0,1,...,n_
 * 该函数可以通过绘制图像来理解（均匀b样条）
 * 注意，当 index < p_ 时，返回的时间可能是负数
 */
inline double UniformBspline::getControlPtMaxT(int index)
{
  if (index < 0) index = 0;
  if (index > n_) index = n_;

  return (double(p_ + 1) / 2.0 + index - p_) * interval_;
}

inline double UniformBspline::getControlPtMaxT(
    int index, int degree, double interval)
{
  return (double(degree + 1) / 2.0 + index - degree) * interval;
}

} // namespace ego_planner
#endif